/*
 * This is the code for the Front card in the Hybrid Autorickshaw
 * project.
 * 
 * Functionality is to control the frontal lights using CAN.
 * 
 * The lights are:
 * Left Full Beam		O	PC5
 * Left Dipped Beam		O	PB2
 * Left Indicator		O	PC4
 * 
 * Right Full Beam		O	PC4
 * Right Dipped Beam	O	PC6
 * Right indicator		O	PB3
 * 
 * Todo:
 * Test everything, and make the warninglight functionality
 */


// Global tinyTimer includes 
#include <tT.h>
#include <env.h>

// Specific includes
#include "can.h"
#include "front.h"

front_t front = {tt_object(),OFF,OFF,OFF,OFF};

//Some functions
void frontCan(front_t *self, can_message_t *msg);

//Make a can listener for this node
can_listener_t listener_front = {(tt_object_t*)&front, (tt_method_t)frontCan, ENV_MSEC(10), 0x700, 0x7f0};

/*
 * Function to initialise the card
 */
void frontInit(front_t *self, void *none){
	//Enabling Lamps as outputs (PB2,PB3,PB4,PC4,PC5,PC6)
	DDRB	|= (1 << DDB2) | (1 << DDB3) | (1 << DDB4);
	DDRC	|= (1 << DDC4) | (1 << DDC5) | (1 << DDC6);
	
	//We always want to start with the dipped beams on
	PORTB |= (1<<PINB2); 
	PORTC |= (1<<PINC6); 
	
	can_listener_t * l = &listener_front;
	TT_SYNC(&can, canInstall, &l);
}

/*
 Translates the can message to the commands 
 */
void frontCan(front_t *self, can_message_t *msg) {
	switch (msg->id & 0xF) {
		case 0x1:
			self->fullBeam = ON; //Remove?
			TT_ASYNC(self,fullBeam,TT_ARGS_NONE);
			break;
		case 0x2:
			self->fullBeam = OFF; //Remove
			TT_ASYNC(self,fullBeam,TT_ARGS_NONE);
			break;
		case 0x3:
			//Toggle the ON/OFF state
			if(!self->leftIndicator){
				self->leftIndicator = ON;
				TT_AFTER(ENV_MSEC(500),self,leftFrontIndicator,TT_ARGS_NONE);
			}				
			break;
		case 0x4:
			if(!self->rightIndicator){
				self->rightIndicator = ON;
				TT_AFTER(ENV_MSEC(500),self,rightFrontIndicator,TT_ARGS_NONE);
			}
			break;
		case 0x5:
			self->rightIndicator 	= OFF;
			self->leftIndicator		= OFF;
			if(!self->warningLights){
				PORTB &= ~(1<<PINB3);
				PORTC &= ~(1<<PINC4);
			}				
			TT_ASYNC(self,rightFrontIndicator,TT_ARGS_NONE);	//Make sure the lights are off
			TT_ASYNC(self,leftFrontIndicator,TT_ARGS_NONE);		//Make sure the lights are off
			break;
		case 0x6:
			//Make an asynchronys call to toggle Emergency fronts
			if(!self->warningLights){
				TT_ASYNC(self,frontWarningLights,TT_ARGS_NONE);
				self->warningLights = ON;
			}			
			break;
		case 0x7:
			self->warningLights = OFF;
			break;
	}
}
/*
 *Function to toggle Full Lights 
 */
void fullBeam(front_t *self, void *none){
	if(self->fullBeam == ON){
		//Fullbeam
		PORTC |= (1<<PINC5); //Left
		PORTB |= (1<<PINB4); //Right
		//Halfbeam
		PORTC	&= ~(1<<PINC6); //Left
		PORTB	&= ~(1<<PINB2);	//Right
	}else{
		//Fullbeam
		PORTB &= ~(1<<PINB4); 
		PORTC &= ~(1<<PINC5);
		//Halfbeam
		PORTB |= (1<<PINB2); 
		PORTC |= (1<<PINC6); 
	}
}

/*
 *Function to toggle right Indicator 
 */
void rightFrontIndicator(front_t *self, void *none){
	if(self->rightIndicator == ON && self->warningLights == OFF){
		PORTB ^= (1<<PINB3); 
		TT_AFTER(ENV_MSEC(500),self,rightFrontIndicator,TT_ARGS_NONE);
	}else{
		//Make sure that the lights will stay off when we stop
		if(!self->warningLights)
			PORTB &= ~(1<<PINB3); 
	}
}

/*
 *Function to toggle left Indicator 
 */
void leftFrontIndicator(front_t *self, void *none){
	if(self->leftIndicator == ON && self->warningLights == OFF){
		PORTC ^= (1<<PINC4); 
		TT_AFTER(ENV_MSEC(500),self,leftFrontIndicator,TT_ARGS_NONE);
	}else{
		//Make sure that the lights will stay off when we stop
		if(!self->warningLights)
			PORTC &= ~(1<<PINC4);
	}
}

void frontWarningLights(front_t *self, void *none){
	if(self->warningLights == ON){
		PORTC ^= (1<<PINC4);
		PORTB ^= (1<<PINB3);
		//TT_AFTER_BEFORE(ENV_MSEC(500),ENV_MSEC(50),self,frontWarningLights,TT_ARGS_NONE);
	}else{
		PORTC &= ~(1<<PINC4);
		PORTB &= ~(1<<PINB3);
	}
}
